//
// Created by sun on 2020/5/28.
//

#include <mavros_msgs/PositionTarget.h>
#include <geometry_msgs/PoseStamped.h>
#include "ros/ros.h"


float pos_sp[3] = {0,0,0.7};
mavros_msgs::PositionTarget pos_setpoint;
geometry_msgs::PoseStamped local_pos_enu;

int main(){
    int argc;char ** argv{nullptr};
    ros::init(argc, argv, "drone_name");
    ros::NodeHandle nh;
    ros::Publisher setpoint_raw_local_pub;
    ros::Publisher vision_pos_pub;
    setpoint_raw_local_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 1);
    vision_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 1);
//Bitmask toindicate which dimensions should be ignored(1 means ignore,0 means not ignore; Bit 10 must set to 0)
//Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7:ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11:yaw, bit 12:yaw_rate
//Bit 10 should set to 0, means is not force sp
    ros::Rate loop_rate(30);
    while(ros::ok()){
        local_pos_enu.pose.position.x =0;
        local_pos_enu.pose.position.y =0;
        local_pos_enu.pose.position.z =0;
        local_pos_enu.header.stamp = ros::Time::now();
        local_pos_enu.pose.orientation.x = 0;
        local_pos_enu.pose.orientation.y = 0;
        local_pos_enu.pose.orientation.z = 0;
        local_pos_enu.pose.orientation.w = 0;
        vision_pos_pub.publish(local_pos_enu);
        pos_setpoint.type_mask = 0b110111111000;  // 100 111 111 000  xyz + yaw
        pos_setpoint.coordinate_frame = 1;
        pos_setpoint.position.x = pos_sp[0];
        pos_setpoint.position.y = pos_sp[1];
        pos_setpoint.position.z = pos_sp[2];
        setpoint_raw_local_pub.publish(pos_setpoint);
        loop_rate.sleep();
    }

}
